#ifndef RRT_PLANNER_H
#define RRT_PLANNER_H

#include "Collision_detector.h"
#include "Sample_tree.h"
#include "Line.h"
#include <time.h>

class Rrt_planner
{
public:
	Rrt_planner (Collision_detector* col, Collision_detector* buff_col,Point start,
				 Point target, int iteration_number,int connect_line_rate,
				 double line_step,int path_num):
		m_start(start), m_target(target), m_iteration_number(iteration_number), 
		m_collision_s(col),m_collision_t(col),m_connect_line_rate(connect_line_rate),
		m_line_step(line_step),m_buff_col(buff_col),m_deg_turn(0.013)
	{	
		time_t seed = time(NULL);
		srand ((unsigned int) seed + 11*path_num);
		m_source_t = new Sample_tree(m_start);
		m_target_t = new Sample_tree(m_target);
	}

	// runs the construction of RRT and returns a path
	list<Point> run()
	{
 		Line source_line = Line  (m_collision_s,m_start,m_line_step);
		Line target_line = Line  (m_collision_t,m_target,m_line_step);

		bool target_connection;
		bool line_connection;

		Point point_rand_s;
		Point point_rand_t;
		Point reach_s;
		Point reach_t;

		for (int i = 0; i < m_iteration_number; i++)
		{

			// check whether an attempt to connect the tree to the target should be made

			line_connection = ((i % m_connect_line_rate) == 0);
			
			// draw a random sample or try to extend towards the target
			if(line_connection && m_source_t->need_line() && source_line.getSize()!=0)
			{
				point_rand_s = source_line.get_rand_point();
			}
			else
			{
				point_rand_s = sample_configuration();
			}

			reach_s = add_point(m_source_t,&m_collision_s,point_rand_s);
			point_rand_t = reach_s;

			if (line_connection && m_target_t->need_line() && target_line.getSize()!=0)
			{
				point_rand_t = target_line.get_rand_point();
			}
			reach_t = add_point(m_target_t,&m_collision_t,point_rand_t);
			if(reach_s[0] == reach_t[0] && reach_s[1] == reach_t[1])
			{
				return get_paths(reach_s,reach_t);
			}
		}
		return list<Point>();
	}

private:

	/*
		Sample a configuration, which doesn't have 
		to be free.
	*/
	Point	sample_configuration()
	{
		double x = ((double)rand())/((double)RAND_MAX) * 2 - 1;
		double y = ((double)rand())/((double)RAND_MAX) * 2 - 1;
		double theta = ((double)rand())/((double)RAND_MAX) * 2 - 1;

		Point result(x,y,theta);

		return result;
	}

	/**
	 * add_point(Sample_tree* active , Collision_detector** col, Point point_rand)
	 *	try to add point_rand to tree or the nearst possible
	 *	return the point added to the tree
	 */
	Point add_point(Sample_tree* active , Collision_detector** col, Point point_rand)
	{
		// find the nearest neighbor in tree
		Point point_nearest = active->nearest(point_rand);

		// try to move toward the new sample from the nearest neighbor
		Point point_new = (*col)->steer(point_nearest, point_rand);

		if (point_new != point_nearest || point_new == point_rand)
		{
			// if this vertex was not considered before (its id is -1)
			if(active->need_line() && fabs(point_new[2]-point_nearest[2])>m_deg_turn)
			{// consider change on about 5 degree
				active->line_off();
				*col = m_buff_col;
			}
			//(get_id(point_new) == -1) add point
			active->add_vertex(point_new);
			point_new = active->get_last_point(); // to make sure that we point_new has a proper id
			active->add_edge(point_nearest, point_new);
		}
		else
		{
			return point_nearest;
		}
		return active->get_last_point();
	}

	/**
	 * get_paths(Point p_s,Point p_t)
	 *	return the union of the pathes of the two trees
	 */
	list<Point>	get_paths(Point p_s,Point p_t)
	{//TODO
		list<Point> path;
		path = m_source_t->get_path(p_s);

		list<Point> t_path = m_target_t->get_path(p_t,false);
		
		t_path.push_back(t_path.back());
		double ang1 = (t_path.front())[2];
		double ang2;
		for (std::list<Point>::iterator it = t_path.begin(); it != t_path.end(); it++)
		{//pushing the angle one position down for every position to orginzie the turn of the robot
			ang2 = it->at(2);
			it->set_angle(ang1);
			ang1=ang2;
		}
		//if we dont push the angle the robot will advence and then turn
		//whcich mean he wiil advence to the rong direction
		
		path.insert(path.end(), t_path.begin(), t_path.end());
		
		return path;
	}

	// parameters
	int m_iteration_number;							// number of iterations of algorithm
	
	int m_connect_line_rate;						// the rate of attempting to connect one of the point in the line to tree

	double m_line_step;								// the space between two points in the line

	Point m_start,m_target;							// start and target configurations

	Collision_detector*			m_collision_s;		//the source tree Collision_detector 

	Collision_detector*			m_collision_t;		//the target tree Collision_detector 
	
	Collision_detector*			m_buff_col;			//Collision_detector of the buffer robot

	Sample_tree*				m_target_t;			//the source tree
	
	Sample_tree*				m_source_t;			//the target tree

	double						m_deg_turn;			//the amount of degree that the robot is consider free to move and turn
};

#endif